#!/usr/bin/python

import roslib; roslib.load_manifest('framer')
import rospy,smach
from geometry_msgs.msg import PointStamped
from framer.msg import Status

class IdleState(smach.State):
    def __init__(self):
        smach.State.__init__(
            self,
            outcomes=['idle', 'prepare'],
            output_keys=['picture_center'],
            input_keys=[]
        )
        self.goals = []
        self.status_pub = rospy.Publisher('/framer/status', Status)
        rospy.Subscriber("/framer/look", PointStamped, self.recordPoint)

    def recordPoint(self, point):
        rospy.loginfo("Received a point to start the photographer.")
        self.goals.append(point)

    def execute(self, data):
        self.status_pub.publish(Status(Status.IDLE))
        if len(self.goals) > 0:
            rospy.loginfo("Initiating photographing.")
            data.picture_center = self.goals.pop()
            return 'prepare'
        return 'idle'
